﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Axiom.Math;
using Axiom.Graphics;

namespace VisualDM.Model
{
    public class Utility
    {
        public const float ZERO_TOLERANCE = 1e-06f;

        static Vector3 UnitCross(Vector3 m_afTuple1, Vector3 m_afTuple2)
        {
            Vector3 kCross = new Vector3(m_afTuple1.y * m_afTuple2.z - m_afTuple1.z * m_afTuple2.y,
                                         m_afTuple1.z * m_afTuple2.x - m_afTuple1.x * m_afTuple2.z,
                                         m_afTuple1.x * m_afTuple2.y - m_afTuple1.y * m_afTuple2.x);
            kCross.Normalize();
            return kCross;
        }

        static void ProjectOntoAxis(TriangleVertices rkTri, Vector3 rkAxis, out float rfMin, out float rfMax)
        {
            float fDot0 = rkAxis.Dot(rkTri.Vertices[0]);
            float fDot1 = rkAxis.Dot(rkTri.Vertices[1]);
            float fDot2 = rkAxis.Dot(rkTri.Vertices[2]);

            rfMin = fDot0;
            rfMax = rfMin;

            if (fDot1 < rfMin)
            {
                rfMin = fDot1;
            }
            else if (fDot1 > rfMax)
            {
                rfMax = fDot1;
            }

            if (fDot2 < rfMin)
            {
                rfMin = fDot2;
            }
            else if (fDot2 > rfMax)
            {
                rfMax = fDot2;
            }
        }


        public static bool Intersection2Triangles(TriangleVertices triangle1, TriangleVertices triangle2)
        {

            // get edge vectors for triangle0
            Vector3[] v = new Vector3[3];
            v[0] = triangle1.Vertices[1] - triangle1.Vertices[0];
            v[1] = triangle1.Vertices[2] - triangle1.Vertices[1];
            v[2] = triangle1.Vertices[0] - triangle1.Vertices[2];
            TriangleVertices akE0 = new TriangleVertices(v);

            // get normal vector of triangle0
            Vector3 kN0 = UnitCross(akE0.Vertices[0], akE0.Vertices[1]);

            // project triangle1 onto normal line of triangle0, test for separation
            float fN0dT0V0 = kN0.Dot(triangle1.Vertices[0]);

            float fMin1, fMax1;
            ProjectOntoAxis(triangle2, kN0, out fMin1, out fMax1);
            if ((fN0dT0V0 < fMin1) || (fN0dT0V0 > fMax1))
            {
                return false;
            }



            // get edge vectors for triangle1

            v = new Vector3[3];
            v[0] = triangle2.Vertices[1] - triangle2.Vertices[0];
            v[1] = triangle2.Vertices[2] - triangle2.Vertices[1];
            v[2] = triangle2.Vertices[0] - triangle2.Vertices[2];
            TriangleVertices akE1 = new TriangleVertices(v);

            // get normal vector of triangle1
            Vector3 kN1 = UnitCross(akE1.Vertices[0], akE1.Vertices[1]);


            Vector3 kDir;
            float fMin0, fMax0;
            int i0, i1;

            Vector3 kN0xN1 = UnitCross(kN0, kN1);

            if (kN0xN1.Dot(kN0xN1) >= ZERO_TOLERANCE)
            {
                // triangles are not parallel

                // Project triangle0 onto normal line of triangle1, test for separation.
                float fN1dT1V0 = kN1.Dot(triangle2.Vertices[0]);
                ProjectOntoAxis(triangle1, kN1, out fMin0, out fMax0);
                if ((fN1dT1V0 < fMin0) || (fN1dT1V0 > fMax0))
                {
                    return false;
                }

                // directions E0[i0]xE1[i1]
                for (i1 = 0; i1 < 3; i1++)
                {
                    for (i0 = 0; i0 < 3; i0++)
                    {
                        kDir = UnitCross(akE0.Vertices[i0], akE1.Vertices[i1]);
                        ProjectOntoAxis(triangle1, kDir, out fMin0, out fMax0);
                        ProjectOntoAxis(triangle2, kDir, out fMin1, out fMax1);
                        if (fMax0 < fMin1 || fMax1 < fMin0)
                        {
                            return false;
                        }
                    }
                }
            }
            else  // triangles are parallel (and, in fact, coplanar)
            {
                // directions N0xE0[i0]
                for (i0 = 0; i0 < 3; i0++)
                {
                    kDir = UnitCross(kN0, akE0.Vertices[i0]);
                    ProjectOntoAxis(triangle1, kDir, out fMin0, out fMax0);
                    ProjectOntoAxis(triangle2, kDir, out fMin1, out fMax1);
                    if (fMax0 < fMin1 || fMax1 < fMin0)
                    {
                        return false;
                    }
                }

                // directions N1xE1[i1]
                for (i1 = 0; i1 < 3; i1++)
                {
                    kDir = UnitCross(kN1, akE1.Vertices[i1]);
                    ProjectOntoAxis(triangle1, kDir, out fMin0, out fMax0);
                    ProjectOntoAxis(triangle2, kDir, out fMin1, out fMax1);
                    if (fMax0 < fMin1 || fMax1 < fMin0)
                    {
                        return false;
                    }
                }
            }
            return true;
        }
    }

    public class AABB
    {
        public Vector3 Min=new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
        public Vector3 Max=new Vector3(-float.MaxValue, -float.MaxValue, -float.MaxValue);
        private Vector3[] corners = new Vector3[8];

        public AABB Transform(Matrix4 matrix)
        {
            Vector3 min = new Vector3();
            Vector3 max = new Vector3();
            Vector3 temp = new Vector3();

            bool isFirst = true;
            int i;
            UpdateCorners();

            for (i = 0; i < corners.Length; i++)
            {
                // Transform and check extents
                temp = matrix * corners[i];
                if (isFirst || temp.x > max.x)
                    max.x = temp.x;
                if (isFirst || temp.y > max.y)
                    max.y = temp.y;
                if (isFirst || temp.z > max.z)
                    max.z = temp.z;
                if (isFirst || temp.x < min.x)
                    min.x = temp.x;
                if (isFirst || temp.y < min.y)
                    min.y = temp.y;
                if (isFirst || temp.z < min.z)
                    min.z = temp.z;

                isFirst = false;
            }
            return new AABB(min, max);
        }

        /// <summary>
        /// 
        /// </summary>
        private void UpdateCorners()
        {
            // The order of these items is, using right-handed co-ordinates:
            // Minimum Z face, starting with Min(all), then anticlockwise
            //   around face (looking onto the face)
            // Maximum Z face, starting with Max(all), then anticlockwise
            //   around face (looking onto the face)
            corners[0] = Min;
            corners[1].x = Min.x;
            corners[1].y = Max.y;
            corners[1].z = Min.z;
            corners[2].x = Max.x;
            corners[2].y = Max.y;
            corners[2].z = Min.z;
            corners[3].x = Max.x;
            corners[3].y = Min.y;
            corners[3].z = Min.z;

            corners[4] = Max;
            corners[5].x = Min.x;
            corners[5].y = Max.y;
            corners[5].z = Max.z;
            corners[6].x = Min.x;
            corners[6].y = Min.y;
            corners[6].z = Max.z;
            corners[7].x = Max.x;
            corners[7].y = Min.y;
            corners[7].z = Max.z;
        }


        public AABB(Vector3 _Min,Vector3 _Max)
        {
            Min=_Min;
            Max=_Max;
            UpdateCorners();
        }

        public AABB(List<TriangleVertices> tri)
        {
            foreach(TriangleVertices tr in tri)
                EnlargeMe(tr);
            UpdateCorners();
        }

        public Matrix4 TransformMatrix;
        private Vector3 transformMin { get { return TransformMatrix * Min; } }
        private Vector3 transformMax { get { return TransformMatrix * Max; } }

        public Vector3 Size{get{return Max-Min;}}
        public Vector3 HalfSize{get{return ((transformMax-transformMin)*0.5f);}}

        public void EnlargeMe(TriangleVertices Triangle )
        {
            for (int i = 0; i < 3; i++)
            {
                Min.x = Math.Min(Min.x, Triangle.Vertices[i].x);
                Min.y = Math.Min(Min.y, Triangle.Vertices[i].y);
                Min.z = Math.Min(Min.z, Triangle.Vertices[i].z);

                Max.x = Math.Max(Max.x, Triangle.Vertices[i].x);
                Max.y = Math.Max(Max.y, Triangle.Vertices[i].y);
                Max.z = Math.Max(Max.z, Triangle.Vertices[i].z);
            }
        }

        public int LongestAxis()
        {
            double dx = Max.x - Min.x;
            double dy = Max.y - Min.y;
            double dz = Max.z - Min.z;

            if (dx >= dy)
            {
                if (dx >= dz)
                {
                    return 0;
                }
                else // dz>dx and dx>=dy
                {
                    return 2;
                }
            }
            else // dy>dx
            {
                if (dy >= dz)
                {
                    return 1;
                }
                else  // dz>dy and dy>dx
                {
                    return 2;
                }
            }
    	}

        private static bool AABBIntersectAABB(AxisAlignedBox a, AxisAlignedBox b)
        {
            return Axiom.Math.Utility.Intersects(a, b) != Intersection.None;
        }

        public bool TestIntersect(AABB box)
        {
            // Exit with no intersection if separated along an axis
            AABB a11 = Transform(TransformMatrix);
            AABB a22 = box.Transform(box.TransformMatrix);
            AxisAlignedBox a1 = new AxisAlignedBox(a11.Min, a11.Max);
            AxisAlignedBox a2 = new AxisAlignedBox(a22.Min, a22.Max);
            return AABBIntersectAABB(a1, a2);
        }
    }

    public class AABBTreeNode
    {
        public List<TriangleVertices> ObjectTriangles = null; //triangle
        public AABB ObjectAABB = null; //aabb
        public AABBTreeNode Positive = null;
        public AABBTreeNode Negative = null;
        
        public AABBTreeNode(AABBTreeNode p,AABBTreeNode n,AABB aabbEl,List<TriangleVertices> triangles)
        {
            Positive = p;
            Negative = n;
            ObjectAABB = aabbEl;
            ObjectTriangles=triangles;
        }

        public bool IsLeaf()
        {
            return ((Negative==null)&&(Positive==null)); 
        }

        private static int SortCompareX(TriangleVertices x1, TriangleVertices x2)
        {
            if (x1.Vertices[0].x < x2.Vertices[0].x)
                return -1;
            else
                if (x1.Vertices[0].x > x2.Vertices[0].x)
                    return 1;
                else return 0;
        }

        private static int SortCompareY(TriangleVertices x1, TriangleVertices x2)
        {
            if (x1.Vertices[0].y < x2.Vertices[0].y)
                return -1;
            else
                if (x1.Vertices[0].y > x2.Vertices[0].y)
                    return 1;
                else return 0;
        }

        private static int SortCompareZ(TriangleVertices x1, TriangleVertices x2)
        {
            if (x1.Vertices[0].z < x2.Vertices[0].z)
                return -1;
            else
                if (x1.Vertices[0].z > x2.Vertices[0].z)
                    return 1;
                else return 0;
        }


        public static AABBTreeNode Build(List<TriangleVertices> triangles)
        {
            if (triangles.Count == 0)
                return null;
            if (triangles.Count == 1)
            {
                AABB box = new AABB(triangles);
                AABBTreeNode Node = new AABBTreeNode(null, null,box, triangles);
                return Node;
            }
            else 
            if (triangles.Count==2)
            {

                AABB box = new AABB(new Vector3(float.MaxValue, float.MaxValue, float.MaxValue), new Vector3(-float.MaxValue, -float.MaxValue, -float.MaxValue));
                foreach (TriangleVertices tr in triangles)
                {
                    box.EnlargeMe(tr);
                }

                List<TriangleVertices> tr1 = new List<TriangleVertices>();
                tr1.Add(triangles[0]);
                List<TriangleVertices> tr2 = new List<TriangleVertices>();
                tr2.Add(triangles[1]);

                AABBTreeNode PosNode = Build(tr1);
                AABBTreeNode NegNode = Build(tr2);

                AABBTreeNode node = new AABBTreeNode(PosNode, NegNode, box,triangles);
                return node;
            }
            else
            if (triangles.Count==3)
            {
                AABB box = new AABB(new Vector3(float.MaxValue, float.MaxValue, float.MaxValue), new Vector3(-float.MaxValue, -float.MaxValue, -float.MaxValue));
                foreach (TriangleVertices tr in triangles)
                {
                    box.EnlargeMe(tr);
                }

                List<TriangleVertices> tr1 = new List<TriangleVertices>();
                tr1.Add(triangles[0]);
                List<TriangleVertices> tr2 = new List<TriangleVertices>();
                tr2.Add(triangles[1]);
                tr2.Add(triangles[2]);

                AABBTreeNode PosNode = Build(tr2);
                AABBTreeNode NegNode = Build(tr1);

                AABBTreeNode node = new AABBTreeNode(PosNode, NegNode, box,triangles);
                return node;
            }
            else
            {

                // Вычисляем "бокс"
                AABB box = new AABB(new Vector3(float.MaxValue, float.MaxValue, float.MaxValue), new Vector3(-float.MaxValue, -float.MaxValue, -float.MaxValue));
                foreach (TriangleVertices tr in triangles)
                {
                    box.EnlargeMe(tr);
                }

                List<TriangleVertices> leftSide = new List<TriangleVertices>();
                List<TriangleVertices> rightSide = new List<TriangleVertices>();

                int longaxis = box.LongestAxis(); // longest axis
                Vector3 geoavg = new Vector3(1f,1f,1f); // geometric average - midpoint of ALL the triangles

                // go through all tris and calculate the average of the midpoints
                List<TriangleVertices> triList = new List<TriangleVertices>(triangles.AsEnumerable());
                switch (longaxis)
                {
                    case 0: { triList.Sort(SortCompareX); break; }
                    case 1: { triList.Sort(SortCompareY); break; }
                    case 2: { triList.Sort(SortCompareZ); break; }
                }

                int range = triList.Count / 2;
                for (int i=0;i<range;i++)
                {
                    rightSide.Add(triList[i]);
                }

                for (int i = range; i < triList.Count; i++)
                {
                    leftSide.Add(triList[i]);
                }

                AABBTreeNode PosNode = Build(new List<TriangleVertices>(rightSide));
                AABBTreeNode NegNode = Build(new List<TriangleVertices>(leftSide));
                AABBTreeNode node = new AABBTreeNode(PosNode, NegNode, box,triangles);
                return node;
            }
        }

    }

    public class AABBTree
    {
        public AABBTreeNode Root;

        public AABBTree()
        {
            Root = null;
        }

        public void SetTransformMatrix(Matrix4 m)
        {
        }

        public void Build(List<TriangleVertices> triangles)
        {
            Root = null;
            if (triangles.Count != 0)
                Root=AABBTreeNode.Build(new List<TriangleVertices>(triangles));
        }

        private bool RecursiveTestIntersectionLeaf( AABBTreeNode pNode1/*листовой узел*/,AABBTreeNode pNode, Matrix4 m1/*for list node*/, Matrix4 m2 )
        {
            if (pNode.IsLeaf())
            {
                Vector3[] vtr = new Vector3[3];
                vtr[0] = m1 * pNode1.ObjectTriangles[0].Vertices[0];
                vtr[1] = m1 * pNode1.ObjectTriangles[0].Vertices[1];
                vtr[2] = m1 * pNode1.ObjectTriangles[0].Vertices[2];
                TriangleVertices transform_triangle1 = new TriangleVertices(vtr);

                TriangleVertices oldtr = pNode.ObjectTriangles[0];
                Vector3[] vtr2 = new Vector3[3];
                vtr2[0] = m2 * oldtr.Vertices[0];
                vtr2[1] = m2 * oldtr.Vertices[1];
                vtr2[2] = m2 * oldtr.Vertices[2];
                TriangleVertices transform_triangle2 = new TriangleVertices(vtr2);
                return Utility.Intersection2Triangles(transform_triangle1, transform_triangle2);
            }
            else
            {
                pNode1.ObjectAABB.TransformMatrix=m1;
                pNode.ObjectAABB.TransformMatrix=m2;
                if (pNode1.ObjectAABB.TestIntersect(pNode.ObjectAABB))
                    return RecursiveTestIntersection(pNode1, pNode.Positive, m1, m2) || RecursiveTestIntersection(pNode1, pNode.Negative, m1, m2);
            }

          return false;
        }

        // Рекурсивно проверяет пересечение двух деревьев.
        private bool RecursiveTestIntersection(AABBTreeNode pMyNode, AABBTreeNode pOutsideNode, Matrix4 m1/*for myNode*/, Matrix4 m2/*for outsideNode*/)
        {
          if( pMyNode.IsLeaf() )
          {
            // pMyNode-лист, pOutsideNode-узел или лист
              return RecursiveTestIntersectionLeaf(pMyNode, pOutsideNode, m1, m2);
          }
          else
          {
            if( pOutsideNode.IsLeaf() )
            {// pMyNode-узел, pOutsideNode-лист
                return RecursiveTestIntersectionLeaf(/*Triangle*/pOutsideNode, pMyNode, m2, m1);
            }
            else
            {// оба узлa
                pMyNode.ObjectAABB.TransformMatrix = m1;
                pOutsideNode.ObjectAABB.TransformMatrix = m2;
                if (pMyNode.ObjectAABB.TestIntersect(pOutsideNode.ObjectAABB))
                {
                    return RecursiveTestIntersection(pMyNode.Positive, pOutsideNode, m1, m2)
                        || RecursiveTestIntersection(pMyNode.Negative, pOutsideNode, m1, m2);
                }
                else       
              {
                  return false;
              }
            }
          }
          return false;
        }

        public bool TestIntersection(AABBTree tree2,Matrix4 m1, Matrix4 m2)
        {
            if ((Root!=null) && (tree2.Root!=null))
                return RecursiveTestIntersection(Root,tree2.Root,m1,m2);
            return false;
        }
    }
}
